#include "algorithm.h"
#include <math.h>

static const char *TAG = "Algorithm";

void pid_init(pid_control_t *pid, float Kp, float Ki, float Kd,
              float max_output) {
  pid->Kp = Kp;
  pid->Ki = Ki;
  pid->Kd = Kd;
  pid->max_output = max_output;
  pid->integral = 0.0f;
  pid->prev_error = 0.0f;
  pid->prev_derivative = 0.0f;

  pid->tau = 0.02f; // 默认滤波时间常数20ms
}

float pid_update(pid_control_t *pid, float error, float dt) {
  // 检查输入有效性
  if (isnan(error)) {
    return 2.13f;
  } else if (isinf(error)) {
    return 1.34;
  } else if (dt <= 0) {
    return 5.67;
  }
  // === 比例项 ===
  float P = pid->Kp * error;

  // === 积分项（梯形积分）===
  float integral_increment = 0.5f * (error + pid->prev_error) * dt;
  pid->integral += integral_increment;
  float I = pid->Ki * pid->integral;

  // === 微分项（带低通滤波）===
  float derivative = 0.0f;
  if (dt > 1e-6f) { // 避免除以零
    derivative = (error - pid->prev_error) / dt;
  }

  // 应用一阶低通滤波
  float alpha = dt / (pid->tau + dt);
  float filtered_derivative =
      (1.0f - alpha) * pid->prev_derivative + alpha * derivative;
  float D = pid->Kd * filtered_derivative;

  // === PID 输出 ===
  float output = P + I + D;

  // === 输出限幅和抗积分饱和 ===
  if (fabsf(output) > pid->max_output) {
    output = (output > 0) ? pid->max_output : -pid->max_output;
    // 抗饱和：回退当前步长的积分增量
    pid->integral -= integral_increment;
  }

  // 更新状态
  pid->prev_derivative = filtered_derivative;
  pid->prev_error = error;

  return output;
}